#!/usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from trajectory_msgs.msg import MultiDOFJointTrajectory

from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist

    
def traj_callback(traj_msg):
    #msg=MultiDOFJointTrajectory()
    msg=traj_msg
    first_traj_point=msg.points[1]
    vel=first_traj_point.velocities[0]
    print(vel)
    cmd_vel_pub.publish(vel)
    
    
if __name__ == '__main__':
    rospy.init_node('traj2cmd')
    
    # Create a subscriber for the /bebop2/ground_truth/odometry topic
    #rospy.Subscriber('/bebop2/ground_truth/odometry', Odometry, odometry_callback)
    rospy.Subscriber('/hummingbird/full_predicted_state',MultiDOFJointTrajectory,traj_callback)

    cmd_vel_pub=rospy.Publisher('/tello/cmd_vel',Twist,queue_size=10)
    
    rospy.spin()